/*------------------------------------------------------
                app层的电机库驱动函数
@brief functions:
.
.
.
.
@attention:

@auther:LWH Cherno
--------------------------------------------------------*/
#include <main.h>
#include "motor_driver.h"

//=====================================================================================================
static motor_data_t motor_data;
//pid buf
static const float rm3508_speed_pid_buf[3] = {RM3508_SPEED_PID_KP, RM3508_SPEED_PID_KI, RM3508_SPEED_PID_KD};

//
static const float rm6020_position_pid_buf[3] = {RM6020_POSITION_PID_KP, RM6020_POSITION_PID_KI, RM6020_POSITION_PID_KD};

//
static const float rm2006_position_pid_buf[3]={
RM2006_POSITION_PID_KP,RM2006_POSITION_PID_KI,RM2006_POSITION_PID_KD}; 

/*-----------------base init functions
--------------------------------------------------------
@brief:
*/
// get motor measure and pid init
void motor_init(void)
{
    for (motor_index_e index = RM3508_1; index < RM3508_4; index++)
    {
        motor_data.RM3508_measure[index] = p_get_chassis_motor_point(index);
        PID_init(&motor_data.rm3508_speed_pid[index], PID_POSITION, rm3508_speed_pid_buf, RM3508_MAX_OUT, RM3508_MAX_IOUT);
    }

    motor_data.RM6020_measure[RM6020_pitch] = p_get_pitch_motor_point();
    PID_init(&motor_data.rm6020_positon_pid[RM6020_pitch], PID_POSITION, rm6020_position_pid_buf, RM6020_MAX_OUT, RM6020_MAX_IOUT);

    motor_data.RM6020_measure[RM6020_yaw] = p_get_yaw_motor_point();
    PID_init(&motor_data.rm6020_positon_pid[RM6020_yaw], PID_POSITION, rm6020_position_pid_buf, RM6020_MAX_OUT, RM6020_MAX_IOUT);

    motor_data.RM2006_measure[RM2006_trigger] = p_get_trigger_motor_point();
    PID_init(&motor_data.rm2006_positon_pid[RM2006_trigger], PID_POSITION, rm2006_position_pid_buf, RM2006_PID_MAX_OUT, RM2006_PID_MAX_IOUT);
}
/*-----------------app functions
--------------------------------------------------------
@brief:
*/
// The 3508 motor calculates the pid to the speed
void rm3508_speed_cmd(float velocity_1, float velocity_2, float velocity_3, float velocity_4)
{
    PID_calc(&motor_data.rm3508_speed_pid[RM3508_1], motor_data.RM3508_measure[RM3508_1]->speed_rpm, velocity_1);
    PID_calc(&motor_data.rm3508_speed_pid[RM3508_2], motor_data.RM3508_measure[RM3508_2]->speed_rpm, velocity_2);
    PID_calc(&motor_data.rm3508_speed_pid[RM3508_3], motor_data.RM3508_measure[RM3508_3]->speed_rpm, velocity_3);
    PID_calc(&motor_data.rm3508_speed_pid[RM3508_4], motor_data.RM3508_measure[RM3508_4]->speed_rpm, velocity_4);

    CAN_cmd_chassis(motor_data.rm3508_speed_pid[RM3508_1].out, motor_data.rm3508_speed_pid[RM3508_2].out, motor_data.rm3508_speed_pid[RM3508_3].out, motor_data.rm3508_speed_pid[RM3508_4].out);
}

//
void rm6020_position_cmd(float angle1)
{
    
}

//=====================================================================================================

/*-----------------XXX task functions
--------------------------------------------------------
@brief:
*/
//





/*-----------------XXX API_c
--------------------------------------------------------
@brief:
*/
//

/*------------------------end-------------------------*/